// MechAlignDlg.cpp : implementation file
//

#include "stdafx.h"
#include "RigCalib.h"
#include "MechAlignDlg.h"
#include "cvxImage.h"
#include "cvxDeckLinkInputCallback.h"
#include "cvxConcentric.h"
#include "cvxBoard.h"


// CMechAlignDlg dialog

IMPLEMENT_DYNAMIC(CMechAlignDlg, CDialog)

CMechAlignDlg::CMechAlignDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CMechAlignDlg::IDD, pParent)
{
	
}

CMechAlignDlg::~CMechAlignDlg()
{
}

void CMechAlignDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	DDX_Control(pDX, IDC_MA_CONVERGENCE, m_convergence);
	DDX_Control(pDX, IDC_MA_TILT, m_tilt);
	DDX_Control(pDX, IDC_MAT_ROLL, m_roll);
	DDX_Control(pDX, IDC_SLIDER_V_OFFSET, m_alignVerticalOffsetSlider);
	DDX_Control(pDX, IDC_SLIDER_EDIT_V_OFF, m_alignVerOffsetEdit);
	DDX_Control(pDX, IDC_SLIDER_ALIGN_HORIZONTAL_OFFSET, m_alignHorizontalOffsetSlider);
	DDX_Control(pDX, IDC_EDIT_ALIGN_HORIZONTAL_OFFSET, m_alignHorizontalOffsetEdit);
	DDX_Control(pDX, IDC_SLIDER_ALIGN_ZOOM, m_alignZoomSlider);
	DDX_Control(pDX, IDC_EDIT_ALIGN_ZOOM, m_alignZoomEdit);
	DDX_Control(pDX, IDC_EDIT_ALIGN_ROTATION, m_alignRotationEdit);
	DDX_Control(pDX, IDC_SLIDER_ALIGN_ROTATION, m_alignRotationSlider);
//	DDX_Control(pDX, IDC_EDIT_ALIGN_KEYSTONE, m_alignKeystoneEdit);
//	DDX_Control(pDX, IDC_SLIDER_ALIGN_KEYSTONE, m_alignKeystoneSlider);
	DDX_Control(pDX, IDC_MA_IA, m_editInterAxisDistance);
	DDX_Control(pDX, IDC_MECH_REPROJ_LEFT, m_projErrLeft);
	DDX_Control(pDX, IDC_MECH_REPROJ_RIGHT, m_projErrRight);
}


BEGIN_MESSAGE_MAP(CMechAlignDlg, CDialog)
	ON_WM_CTLCOLOR()
	ON_WM_PAINT()
END_MESSAGE_MAP()


// CMechAlignDlg message handlers

BOOL CMechAlignDlg::OnInitDialog()
{
	CDialog::OnInitDialog();

	// TODO:  Add extra initialization here
	// init brushes and colors
	m_redColor  = RGB(255, 0, 0);
	m_greenColor = RGB(0, 255, 0);
	m_redBrush.CreateSolidBrush(m_redColor);
	m_greenBrush.CreateSolidBrush(m_greenColor);

	//get a larger font
	CFont *pfont = m_convergence.GetFont(); 
	pfont->GetLogFont(&m_font); 
	m_font.lfHeight += m_font.lfHeight/2; 
	m_font.lfWidth  += m_font.lfWidth/2; 
	CFont   font; 
	font.CreateFontIndirect(&m_font); 

	//set default parameters
	m_convergence.SetFont(&font);
	m_convergence.SetWindowText("No result!");
	m_tilt.SetFont(&font);
	m_tilt.SetWindowText("No result!");
	m_roll.SetFont(&font);
	m_roll.SetWindowText("No result!");
	m_editInterAxisDistance.SetFont(&font);
	m_editInterAxisDistance.SetWindowText("No result!");

	m_leftImage = NULL;
	m_rightImage = NULL;	
	m_pSideByside = NULL;
	m_pOverlay = NULL;

	//calibration	
	m_readyCalib = false;
	m_sideBysideReady = false;
	m_overlayReady = false;
	m_showIAReady = false;

	//dense calibration
	m_denseCalib = false;; 
	m_board.SetConfigure(cvSize(14, 10), 15.2);	

	//sliders
	m_alignVerticalOffsetSlider.SetRange(-4, 4);
	m_alignVerticalOffsetSlider.SetTicFreq(1);
	m_alignVerticalOffsetSlider.SetPos(0);
	m_alignVerOffsetEdit.SetFont(&font);
	m_alignVerOffsetEdit.SetWindowText("No result!");

	m_alignHorizontalOffsetSlider.SetRange(-4, 4);
	m_alignHorizontalOffsetSlider.SetTicFreq(1);
	m_alignHorizontalOffsetSlider.SetPos(0);
	m_alignHorizontalOffsetEdit.SetFont(&font);
	m_alignHorizontalOffsetEdit.SetWindowText("No result!");

	m_alignZoomSlider.SetRange(-4, 4);
	m_alignZoomSlider.SetTicFreq(1);
	m_alignZoomSlider.SetPos(0);
	m_alignZoomEdit.SetFont(&font);
	m_alignZoomEdit.SetWindowText("No result!");

	m_alignRotationSlider.SetRange(-4, 4);
	m_alignRotationSlider.SetTicFreq(1);
	m_alignRotationSlider.SetPos(0);
	m_alignRotationEdit.SetFont(&font);
	m_alignRotationEdit.SetWindowText("No result!");

// 	m_alignKeystoneSlider.SetRange(-4, 4);
// 	m_alignKeystoneSlider.SetTicFreq(1);
// 	m_alignKeystoneSlider.SetPos(0);
// 	m_alignKeystoneEdit.SetFont(&font);
// 	m_alignKeystoneEdit.SetWindowText("No result!");


	//re projection error
	m_projErrLeft.SetWindowText("No result!");
	m_projErrRight.SetWindowText("No result!");

	return TRUE;  // return TRUE unless you set the focus to a control
	// EXCEPTION: OCX Property Pages should return FALSE
}

HBRUSH CMechAlignDlg::OnCtlColor(CDC* pDC, CWnd* pWnd, UINT nCtlColor)
{
	HBRUSH hbr = CDialog::OnCtlColor(pDC, pWnd, nCtlColor);

	// TODO:  Change any attributes of the DC here
	switch(nCtlColor)
	{
	case CTLCOLOR_EDIT:
		switch(pWnd->GetDlgCtrlID())
		{
			//horizontal offset
		//	pDC->SetBkColor(m_redColor);    // change the background
		//	hbr = (HBRUSH) m_redColor;
			break;

		default:
			break;
		}
		break;
		default:
			break;
	}


	// TODO:  Return a different brush if the default is not desired
	return hbr;
}

void CMechAlignDlg::OnPaint()
{
	CPaintDC dc(this); // device context for painting
	// TODO: Add your message handler code here
	// Do not call CDialog::OnPaint() for painting messages
}

void CMechAlignDlg::updateImages(char *pdata, char *pRightData, int w, int h, int nChannels, bool flipRight)
{
	assert(nChannels == 2);
	assert(pdata);

	//update image formate
	if (!m_leftImage)
	{
		m_leftImage = cvCreateImage(cvSize(w, h), IPL_DEPTH_8U, 1);
	}
	else if (m_leftImage->width != w || m_leftImage->height != h)
	{
		cvReleaseImage(&m_leftImage);
		m_leftImage = cvCreateImage(cvSize(w, h), IPL_DEPTH_8U, 1);
		assert(m_leftImage);
	}

	if (!m_rightImage)
	{
		m_rightImage = cvCreateImage(cvSize(w, h), IPL_DEPTH_8U, 1);
		assert(m_rightImage);
	}
	else if (m_rightImage->width != w || m_rightImage->height != h)
	{
		cvReleaseImage(&m_rightImage);
		m_rightImage = cvCreateImage(cvSize(w, h), IPL_DEPTH_8U, 1);
		assert(m_rightImage);
	}

	//update image data
	cvxYUV422toGray(w, h, (unsigned char*)pdata, m_leftImage);

//	cvShowImage("left image from yuv422", m_leftImage);
//	cvWaitKey(30);

	if (pRightData)
	{
		cvxYUV422toGray(w, h, (unsigned char *)pRightData, m_rightImage);
	}

	//add marker detector
	bool found = trackingMarker();
	if (m_denseCalib == true)
	{
		DenseCalib();
	}
	else
	{
		//calibrate with marker corners
		if (m_readyCalib == true && found)
		{
			//@todo load chessboard information
			Calib();		
		}
	}	
	
	//side by side update images
	if (m_sideBysideReady && m_pSideByside)
	{
		//add showMarker checker
		m_pSideByside->updateImages(pdata, pRightData, w, h, nChannels, m_preLeftMarkers, m_preRightMarkers, flipRight);
	}
	//overlay update image
	if (m_overlayReady && m_pOverlay && pRightData)
	{
		//show red-cyan image
		m_pOverlay->updateImages(pdata, pRightData, w, h, nChannels, m_preLeftMarkers, m_preRightMarkers, flipRight);
	}	
	
	//align
	if (found)
	{
		Align();
	}	
	//@todo add overlay updateimages
//	InvalidateRect(0, FALSE);
}
void CMechAlignDlg::SetCameraConfigure(const SCameraConfigure &left, const SCameraConfigure &right, int w, int h)
{
	m_rig.m_leftEye.setConfigure(left.fx, left.fy, left.cx, left.cy, left.k1, left.k2, left.p1, left.p2, left.k3);
	m_rig.m_rightEye.setConfigure(right.fx, right.fy, right.cx, right.cy, right.k1, right.k2, right.p1, right.p2, right.k3);
	m_rig.m_leftEye.setImageSize(w, h);
	m_rig.m_rightEye.setImageSize(w, h);
	m_rig.m_leftEye.computerPhysicalParameter();
	m_rig.m_rightEye.computerPhysicalParameter();
	m_readyCalib = true;
}

bool CMechAlignDlg::trackingMarker()
{
	//@todo marker criteria checker
	int markerNumber = 4;
	MarkerCriteria criteria;
	criteria._fastCheck = true;
	criteria._minPixels = 100;
	criteria._maxPixels = 800;

	bool leftFound = false;
	bool rightFound = false;
	vector<CvxMarker> leftCurMarkers;
	vector<CvxMarker> rightCurMakers;
	if (m_leftImage)
	{
		if (m_preLeftMarkers.size() == markerNumber)
		{
			leftFound = CvxMarker::tracking(m_leftImage, leftCurMarkers, 
					markerNumber, criteria, m_preLeftMarkers);
		}
		if (!leftFound)
		{
			leftFound = (CVX_MARKER_SUCCESS == 
					CvxMarker::detect(m_leftImage, leftCurMarkers, markerNumber, criteria));
		}		
	}
	//update preMarkers
	m_preLeftMarkers.clear();
	if (leftFound)
	{
		for (int i = 0; i<markerNumber; ++i)
		{
			m_preLeftMarkers.push_back(leftCurMarkers[i]);
		}
	}

	if (m_rightImage)
	{
		if (m_preRightMarkers.size() == markerNumber)
		{

			rightFound = CvxMarker::tracking(m_rightImage, rightCurMakers, 
					markerNumber, criteria, m_preRightMarkers);
		}
		if (!rightFound)
		{
			rightFound = (CVX_MARKER_SUCCESS == 
				CvxMarker::detect(m_rightImage, rightCurMakers, markerNumber, criteria));
		}		
	}
	m_preRightMarkers.clear();
	if (rightFound)
	{
		for (int i = 0; i<markerNumber; ++i)
		{
			m_preRightMarkers.push_back(rightCurMakers[i]);
		}
	}
	return leftFound && rightFound;
}

void CMechAlignDlg::Calib(void)
{
	//@todo	

	CvxMarkerChessboard board;
	board.SetConfigure(cvSize(14, 10), 15.2);
	board.SetMarkerCorners(20.26);
	vector<Point3f> objPts  = board.m_markerCorners3d;


	vector<Point2f> leftImagePts;
	vector<Point2f> rightImagePts;

	//get left and right image points
	for (int i = 0; i<4; ++i)
	{
		for (int j = 0; j<4; ++j)
		{
			Point2f p1 = m_preLeftMarkers[i].m_corners[j];
			Point2f p2 = m_preRightMarkers[i].m_corners[j];
			leftImagePts.push_back(p1);
			rightImagePts.push_back(p2);
		}		
	}

	if(m_rig.GetOrientation(objPts, leftImagePts, rightImagePts))
	{
		//show result
		char rst[4][256] = {NULL};
		sprintf(rst[0], "%f", m_rig.m_eulerAngle[0]);
		sprintf(rst[1], "%f", m_rig.m_eulerAngle[1]);
		sprintf(rst[2], "%f", m_rig.m_eulerAngle[2]);
		m_convergence.SetWindowText(rst[1]);
		m_tilt.SetWindowText(rst[0]);
		m_roll.SetWindowText(rst[2]);

		if (m_showIAReady)
		{
			float ia_dis = m_rig.GetInterAxis(objPts, leftImagePts, rightImagePts, board.GetUnitLength());
			sprintf(rst[3], "%f", ia_dis);
			m_editInterAxisDistance.SetWindowText(rst[3]);
		}
		else
		{
			m_editInterAxisDistance.SetWindowText("No result!");
		}

		m_projErrLeft.SetWindowText("No result!");
		m_projErrRight.SetWindowText("No result!");
		InvalidateRect(0, FALSE);
	}
}

void CMechAlignDlg::DenseCalib()
{
	//get chessboard corners
	vector<Point2f> leftImagePts;
	vector<Point2f> rightImagePts;
	vector<CvxMarker> curLeftMarkers;
	vector<CvxMarker> curRightMarkers;
	bool isLeftOk = false, isRightOk = false;

	//get corners in the left image
	if (m_leftImage)
	{
		isLeftOk = m_board.FindCorners(m_leftImage, true, m_boardPeLeftMarkers, curLeftMarkers, leftImagePts);
		//board has changed the pose
		if (!isLeftOk && !m_boardPeLeftMarkers.empty())
		{
			m_boardPeLeftMarkers.clear();
			isLeftOk = m_board.FindCorners(m_leftImage, false, m_boardPeLeftMarkers, curLeftMarkers, leftImagePts);			
		}
	}

	if (!isLeftOk)
	{
		return;
	}
	else
	{
		m_boardPeLeftMarkers = curLeftMarkers;
		
		
	}

	if (m_rightImage)
	{
		isRightOk = m_board.FindCorners(m_rightImage, true, m_boardPeRightMarkers, curRightMarkers, rightImagePts);
		//board has changed the pose
		if (!isRightOk && !m_boardPeRightMarkers.empty())
		{
			m_boardPeRightMarkers.clear();
			isRightOk = m_board.FindCorners(m_rightImage, false, m_boardPeRightMarkers, curRightMarkers, rightImagePts);			
		}
	}

	if (!isRightOk)
	{
		return;
	}
	else
	{
		m_boardPeRightMarkers = curRightMarkers;		
	}	
	//get chessboard 3d positions
	vector<Point3f> pts3d;
	m_board.Get3Dpts(pts3d);

	

	//get extrinsic parameters
	if(m_rig.GetOrientation(pts3d, leftImagePts, rightImagePts))
	{
		//show result
		char rst[4][256] = {NULL};
		sprintf(rst[0], "%f", m_rig.m_eulerAngle[0]);
		sprintf(rst[1], "%f", m_rig.m_eulerAngle[1]);
		sprintf(rst[2], "%f", m_rig.m_eulerAngle[2]);
		m_convergence.SetWindowText(rst[1]);
		m_tilt.SetWindowText(rst[0]);
		m_roll.SetWindowText(rst[2]);

		m_editInterAxisDistance.SetWindowText("No result!");		
	}

	//get re projection error
	double leftErr = 0, rightErr = 0;
	if (m_rig.GetReprojectionError(pts3d, leftImagePts, rightImagePts, leftErr, rightErr))
 	{
 		char rst[2][256] = {NULL};
 		sprintf(rst[0], "%f", leftErr);
 		sprintf(rst[1], "%f", rightErr);
 		m_projErrLeft.SetWindowText(rst[0]);
 		m_projErrRight.SetWindowText(rst[1]);
 	}

	InvalidateRect(0, FALSE);
	

}

void CMechAlignDlg::Align()
{
	//get four marker centers from two marker sequence
	vector<Point2f> leftpts;
	vector<Point2f> rightpts;
	int w = m_leftImage->width;
	int h = m_leftImage->height;
	float zoomRatio = 0.0f;
	float verticalOffsetPercent = 0.0f;
	float horizontalOffsetPercent = 0.0f;
	float rotationAngle = 0.0f;
	float keystone = 0.0f;
	for (int i = 0; i<m_preLeftMarkers.size(); ++i)
	{
		Point2f p1 = m_preLeftMarkers[i].m_centerPt;
		Point2f p2 = m_preRightMarkers[i].m_centerPt;
		leftpts.push_back(p1);
		rightpts.push_back(p2);
	}
	if (m_readyCalib)
	{
		m_rig.undistortion(leftpts, rightpts);
	}
	cvxAlignment(leftpts, rightpts, w, h, zoomRatio,
				verticalOffsetPercent, horizontalOffsetPercent);
	cvxAlignRotation(leftpts, rightpts, rotationAngle);
//	cvxAlignKeyStone(leftpts, rightpts, keystone);

	//update edit
	char rst[5][256] = {NULL};
	sprintf(rst[0], "%f", verticalOffsetPercent);
	sprintf(rst[1], "%f", horizontalOffsetPercent);
	sprintf(rst[2], "%f%%",zoomRatio);
	sprintf(rst[3], "%f%",rotationAngle);
//	sprintf(rst[4], "%f", keystone);

	m_alignVerOffsetEdit.SetWindowText(rst[0]);
	m_alignHorizontalOffsetEdit.SetWindowText(rst[1]);
	m_alignZoomEdit.SetWindowText(rst[2]);
	m_alignRotationEdit.SetWindowText(rst[3]);
//	m_alignKeystoneEdit.SetWindowText(rst[4]);

	//update slider
	//vertical offset -0.4 to 0.4
	if (verticalOffsetPercent <= -0.4)
	{
		m_alignVerticalOffsetSlider.SetPos(-4);
	}
	else if (verticalOffsetPercent >= 0.4)
	{
		m_alignVerticalOffsetSlider.SetPos(4);
	}
	else
	{
		int pos = cvRound(verticalOffsetPercent * 10);
		assert(pos >= -4 && pos <= 4);
		m_alignVerticalOffsetSlider.SetPos(pos);
	}
	//horizontal offset -0.4 to 0.4
	if (horizontalOffsetPercent <= -0.4)
	{
		m_alignHorizontalOffsetSlider.SetPos(-4);
	}
	else if (horizontalOffsetPercent >= 0.4)
	{
		m_alignHorizontalOffsetSlider.SetPos(4);
	}
	else
	{
		int pos = cvRound(horizontalOffsetPercent*10);
		assert(pos >= -4 && pos <= 4);
		m_alignHorizontalOffsetSlider.SetPos(pos);
	}

	//zoom ratio from 99.6 to 100.4
	if (zoomRatio <= 99.6)
	{
		m_alignZoomSlider.SetPos(-4);
	}
	else if (zoomRatio >= 100.4)
	{
		m_alignZoomSlider.SetPos(4);
	}
	else
	{
		float tZoom = zoomRatio - 100.0f;
		int pos = cvRound(tZoom * 10);
		assert(pos >= -4 && pos <= 4);
		m_alignZoomSlider.SetPos(pos);
	}

	//rotation angle from -0.40 to 0.40
	if (rotationAngle <= -0.40)
	{
		m_alignRotationSlider.SetPos(-4);
	}
	else if (rotationAngle >= 0.40)
	{
		m_alignRotationSlider.SetPos(4);
	}
	else
	{
		int pos = cvRound(rotationAngle * 10);
		assert(pos >= -4 && pos <= 4);
		m_alignRotationSlider.SetPos(pos);
	}

	//keystone -0.4 to 0.4
// 	if (keystone <= -0.4)
// 	{
// 		m_alignKeystoneSlider.SetPos(-4);
// 	}
// 	else if (keystone >= 0.4)
// 	{
// 		m_alignKeystoneSlider.SetPos(4);
// 	}
// 	else
// 	{
// 		int pos = cvRound(keystone * 10);
// 		assert(pos >= -4 && pos <= 4);
// 		m_alignKeystoneSlider.SetPos(pos);
// 	}
	InvalidateRect(0, FALSE);
}
